#include <cstdlib>
#include <iostream>
#include <stdio.h>
#include <time.h>
#include <pthread.h>
#include <sys/siginfo.h>
#include <sys/neutrino.h>
#include <signal.h>
#include <errno.h>
#include <unistd.h>



struct PosVel
{
	double Vx;
	double Vy;
	double Vz;
	double X;
	double Y;
	double Z;
};


float Ax = -2;
float Ay = 4.0;
float Az = 10.5;

int     channel_id_DD;                       // channel ID
int     channel_id_DAP;                       // channel ID

int DAPupdate = 1;
int DDupdate = 3;

pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;
PosVel posVel;

long long millsecDiff(timespec start, timespec end)
{
	

}


void displayData()
{
	printf("\n");
	printf("Vx:%f",posVel.Vx);
	printf(" Vy:%f",posVel.Vy);
	printf(" Vz:%f",posVel.Vz);
	printf(" X:%f", posVel.X);
	printf(" Y:%f", posVel.Y);
	printf(" Z:%f", posVel.Z);

}

void updateData()
{
	posVel.Vx += Ax * DAPupdate;
	posVel.Vy += Ay * DAPupdate;
	posVel.Vz += (Az - 9.81) * DAPupdate;
	posVel.X  += posVel.Vx * DAPupdate; 
	posVel.Y  += posVel.Vy * DAPupdate;
	posVel.Z  += posVel.Vz * DAPupdate;
	
}

bool setUpTimer(unsigned int seconds, int channel)
{
	 timer_t    timerid;    
     sigevent   event;     
     itimerspec timer;  
     int        coid;     
     

     
	     
	// create a connection back to ourselves
	     coid = ConnectAttach (0, 0, channel, 0, 0);
	     
	     if (coid == -1) {
	             printf("Error coid");
	             perror (NULL);
	             return 0;
	         }
	     
	     // set up the kind of event that we want to deliver -- a pulse
	     SIGEV_PULSE_INIT (&event, coid, SIGEV_PULSE_PRIO_INHERIT, 1, 0);
	     
	     if (timer_create (CLOCK_REALTIME, &event, &timerid) == -1) {
	    	 perror(NULL);
	    	 printf("error"); 
	    	 return(0);
	     }

	     
	     // setup the timer (1s delay, 1s reload)
	     timer.it_value.tv_sec = seconds;
	     timer.it_value.tv_nsec = 0;
	     timer.it_interval.tv_sec = seconds;
	     timer.it_interval.tv_nsec = 0;

	     // and start it!
	     timer_settime (timerid, 0, &timer, NULL);
	
	
	return true;
}

void *DAP(void *)
{
    timespec time_pre;
    timespec time_post;
    
	printf("\nDAP thread started");
	if ((channel_id_DAP = ChannelCreate (0)) == -1) {
		         perror (NULL);
		         return 0;
		     }
	     
	    setUpTimer(DAPupdate,channel_id_DAP);
		
		
		
		while(1){
			//lock mutex
			pthread_mutex_lock( &mutex );
			//display data
			updateData();
			//release mutex
			pthread_mutex_unlock( &mutex );
			printf("One DAP loop: %f", millsecDiff(time_pre, time_post) );
			//block until pulse
			MsgReceive (channel_id_DAP, NULL, NULL, NULL);
		};
	
	return NULL;
}

int main(int argc, char *argv[]) {
	//http://www.qnx.com/developers/docs/6.4.1/neutrino/getting_started/s1_timer.html#using
	posVel.X  = 0;
	posVel.Y  = 0;
	posVel.Z  = 0;
	posVel.Vx = 0;
	posVel.Vy = 0;
	posVel.Vz = 0;
	
    timespec time_pre;
    timespec time_post;
	
	
	printf("\nDD thread started");
	pthread_create(NULL, NULL, DAP, NULL);
	
	 if ((channel_id_DD = ChannelCreate (0)) == -1) {
	         perror (NULL);
	         return 0;
	     }

    setUpTimer(DDupdate,channel_id_DD);
	
	while(1){
		clock_gettime( CLOCK_REALTIME, &time_pre);
		
		
		//lock mutex
		pthread_mutex_lock( &mutex );
		//display data
		displayData();
		//release mutex
		pthread_mutex_unlock( &mutex );
		clock_gettime( CLOCK_REALTIME, &time_post);
		printf("One DD loop: %f", millsecDiff(time_pre, time_post) );
		//block until pulse
		MsgReceive (channel_id_DD, NULL, NULL, NULL);
	};

	return EXIT_SUCCESS;
}

